/*
*******************************************************************************
* Copyright (c) 2021 by M5Stack
*                  Equipped with M5Core2 sample source code
*                          配套  M5Core2 示例源代码
* Visit the website for more information: https://docs.m5stack.com/en/module/servo2
* 获取更多资料请访问: https://docs.m5stack.com/zh_CN/module/servo2
*
* describe: servo2.
* date: 2021/9/2
*******************************************************************************
  Use SERVO2 Module to control the rotation of 16-channel servo.
  使用 SERVO2 模块控制 16 通道舵机的旋转。
*/

#include <M5Core2.h>
#include <Wire.h>
#include "Adafruit_PWMServoDriver.h"

Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x40, Wire1);

#define SERVOMIN  102 // This is the 'minimum' pulse length count (out of 4096)  这是“最小”脉冲长度计数（共 4096 个）
#define SERVOMAX  512 // This is the 'maximum' pulse length count (out of 4096)  这是“最大”脉冲长度计数（共 4096 个）
#define USMIN  500 // This is the rounded 'minimum' microsecond length based on the minimum pulse of 102  这是基于 102 的最小脉冲的舍入“最小”微秒长度
#define USMAX  2500 // This is the rounded 'maximum' microsecond length based on the maximum pulse of 512  这是基于 512 的最大脉冲的舍入“最大”微秒长度
#define SERVO_FREQ 50 // Analog servos run at ~50 Hz updates  模拟伺服以 ~50 Hz 更新运行

void setup() {
  M5.begin(true,true,true,true,kMBusModeInput);
/*   kMBusModeOutput,powered by USB or Battery
  kMBusModeInput,powered by outside input need to fill in this Otherwise M5Core2 will not work properly
  由外部供电时此项必填,否则M5Core2将无法正常工作 */
  Wire1.begin(21,22);
  pwm.begin();
  pwm.setPWMFreq(50); 
  M5.Lcd.setCursor(115, 0, 4);
  M5.Lcd.setTextColor(TFT_GREEN, TFT_BLACK);
  M5.Lcd.print("Servo2");
}

void setServoPulse(uint8_t n, double pulse) {
   double pulselength;
   pulselength = 1000000;   // 1,000,000 us per second
   pulselength /= 50;   // 50 Hz
   Serial.print(pulselength); Serial.println(" us per period"); 
   pulselength /= 4096;  // 12 bits of resolution
   Serial.print(pulselength); Serial.println(" us per bit"); 
   pulse *= 1000;
   pulse /= pulselength;
   Serial.println(pulse);
   pwm.setPWM(n, 0, pulse);
}

void servo_angle_write(uint8_t n, int Angle) {
  double pulse = Angle;
  pulse = pulse/90 + 0.5;
  setServoPulse(n, pulse);
}

void loop() {
  for(int i=0; i<16; i++) {
    setServoPulse(i, 0.5);
  }
  delay(500);
  for(int i=0; i<16; i++) {
    setServoPulse(i, 2.5);
  }
  delay(500);
}
